#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@CreateTime: 2025/10/9 10:52
@Author  : AnimateX
@Contact : animatex@163.com
@File    : rmsl_viewer_refactor.py
@License : Copyright © 2025 AnimateX. All rights reserved.
@version : rmsl_viewer_refactor_v2025_10_09

--------------------------------------------------------------

@Description:
    [Update History]: 2.0 Version
        [2025-10-14]: v2.0.0 Dual/Three UVC support(new version)
        [2025-10-20]: V2.0.1 fix exit bug and optim performance analysis
        [2025-10-21]: V2.0.2 隐藏开机过程中的terminal弹窗
        [2025-11-13]: 重构UI部分，支持分辨率选择和帧率展示
        [2025-11-14]: 获取标定文件
"""

from PyQt5 import QtWidgets, QtCore
from PyQt5.QtGui import QPixmap, QImage, QPainter, QIcon, QColor
from PyQt5.QtCore import QTimer, Qt, pyqtSignal, QEvent, QThread, QRectF
from PyQt5.QtWidgets import (QDialog, QProgressDialog,
                             QWidget, QGraphicsScene, QGraphicsView, QApplication,
                             QGraphicsPixmapItem, QToolTip, QMessageBox)
import os
import cv2
import sys
import time
import struct
import subprocess
import numpy as np

from collections import deque
from queue import Queue, Empty, Full
from datetime import datetime

from threading import Thread, Lock, Event
from typing import Dict, Optional, Any

from viewer_new_ui import (Ui_RMSLViewer, PerformanceMonitorDialog, SettingsDialog, STYLE_CONFIG,
                           RMSL_LOG_PATH, init_logger, FFMPEG, PROJECT_ROOT, UtilsPointCloud,
                           UtilsFile, Colorizer, RMSLAlign, FrameData, StreamConfig, MatchedDataPack,
                           RMSLUVCController, UVCFormatEnumerator, CalibrationParser, CalibrationScaler)
import viewer_rc
from pyqtgraph.opengl import GLViewWidget
import pyqtgraph.opengl as gl
import pyqtgraph as pg


""" ------------------------------------------ [有界队列管理器] ------------------------------------------ """
class BoundedQueue:
    """带丢弃策略的有界队列"""
    def __init__(self, maxsize=2, name=""):
        self.queue = Queue(maxsize=maxsize)
        self.name = name
        self._lock = Lock()  # 保护计数器
        self._closed = False

        # 统计信息
        self.put_drop_count = 0  # put时丢弃的帧
        self.get_skip_count = 0  # get_latest跳过的帧
        self.total_put = 0
        self.total_get = 0

    def put(self, item, drop_old=True):
        """
        drop_old=True: 队列满时丢弃最旧数据
        drop_old=False: 队列满时丢弃新数据
        """
        if self._closed:
            return False

        with self._lock:
            if self._closed:  # 双重检查
                return False

            self.total_put += 1

            if self.queue.full():
                if drop_old:
                    try:
                        self.queue.get_nowait()
                        self.put_drop_count += 1
                    except Empty:
                        pass
                else:
                    self.put_drop_count += 1
                    return False

            try:
                self.queue.put_nowait(item)
                return True
            except Full:
                return False

    def get_latest(self, timeout=0.01):
        if self._closed:
            return None

        try:
            item = self.queue.get(timeout=timeout)
        except Empty:
            return None

        skipped = 0
        while True:
            try:
                new_item = self.queue.get_nowait()
                skipped += 1
                item = new_item  # 更新为更新的数据
            except Empty:
                break

        with self._lock:
            self.total_get += 1
            if skipped > 0:
                self.get_skip_count += skipped

        return item

    def get(self, timeout=None):
        """常规获取（FIFO）"""
        if self._closed:
            return None

        try:
            item = self.queue.get(timeout=timeout)
            with self._lock:
                self.total_get += 1
            return item
        except Empty:
            return None

    def qsize(self):
        return self.queue.qsize()

    def clear(self):
        with self._lock:
            while not self.queue.empty():
                try:
                    self.queue.get_nowait()
                except Empty:
                    break

    def close(self):
        self._closed = True
        time.sleep(0.05)
        self.clear()

    def reset(self):
        """重置队列为可用状态（用于重启）"""
        self.clear()
        with self._lock:
            self._closed = False
            self.put_drop_count = 0
            self.get_skip_count = 0
            self.total_put = 0
            self.total_get = 0

    @property
    def is_closed(self):
        return self._closed

    def get_stats(self):
        with self._lock:
            total_drops = self.put_drop_count + self.get_skip_count
            return {
                'name': self.name,
                'qsize': self.queue.qsize(),
                'put_drops': self.put_drop_count,
                'get_skips': self.get_skip_count,
                'total_drops': total_drops,
                'total_put': self.total_put,
                'total_get': self.total_get,
                'drop_rate': total_drops / self.total_put if self.total_put > 0 else 0
            }

""" ------------------------------------------ [MessageBox美化] ------------------------------------------ """
MessageBox_Style = """
    QMessageBox {
        background-color: #F5F5F5;
        font-family: 'YaHei Consolas Hybrid', 'Microsoft YaHei', sans-serif;
        font-size: 10px;
        padding: 10px;
        spacing: 8px;
    }
    QMessageBox QLabel {
        color: #333333;
        font-size: 12px;
        min-width: 0px;
        margin-left: 0px;
        padding-left: 0px;
    }
    QMessageBox::icon-label {
        min-width: 32px;
        min-height: 32px;
        margin-right: 0px;
        padding: 0px;
        qproperty-alignment: AlignCenter;
    }
    QMessageBox QPushButton {
        background-color: #4A90E2;
        font-family: 'YaHei Consolas Hybrid', 'Microsoft YaHei', sans-serif;
        font-size: 12px;
        color: white;
        border-radius: 4px;
        padding: 5px 15px;
        outline: none;
        border: none;
        min-width: 80px;
        min-height: 25px;
    }
    QMessageBox QPushButton:hover {
        background-color: #5A9AE8;
    }
    QMessageBox QPushButton:pressed {
        background-color: #3A80D2;
    }
"""

""" ------------------------------------------ [RGB/Depth匹配器] ------------------------------------------ """
class SensorDataMatcher:
    def __init__(self, max_time_diff=200, max_queue_size=20):
        self.rgb_buffer = deque(maxlen=max_queue_size)
        self.depth_buffer = deque(maxlen=max_queue_size)
        self.max_time_diff = max_time_diff
        self.max_queue_size = max_queue_size
        self.lock = Lock()
        self.logger = init_logger(RMSL_LOG_PATH)

        self.matched_count = 0
        self.rgb_dropped = 0
        self.depth_dropped = 0

        # update flag
        self.depth_updated = False

    def add_rgb_data(self, img_rgb, timestamp):
        with self.lock:
            data = {'img': img_rgb, 'timestamp': timestamp}
            self.rgb_buffer.append(data)
            if len(self.rgb_buffer) > self.max_queue_size:
                self.rgb_buffer.popleft()
                self.rgb_dropped += 1

    def add_depth_data(self, img_disp, img_depth, timestamp, pse_color_depth_img=None):
        with self.lock:
            data = {
                'disp': img_disp,
                'depth': img_depth,
                'pse_color': pse_color_depth_img,
                'timestamp': timestamp
            }
            self.depth_buffer.append(data)
            if len(self.depth_buffer) > self.max_queue_size:
                self.depth_buffer.popleft()
                self.depth_dropped += 1

            self.depth_updated = True

    def find_best_match(self):
        with self.lock:
            if not self.rgb_buffer or not self.depth_buffer:
                self.logger.log_to_file(f" ⚠️ [Warn] Frame buffer empty!")
                time.sleep(0.01)
                return None, None

            if not self.depth_updated:
                time.sleep(0.01)
                return None, None

            latest_depth = self.depth_buffer[-1]
            target_timestamp = latest_depth['timestamp']

            best_rgb = None
            best_rgb_index = -1
            min_time_diff = float('inf')

            for i in range(len(self.rgb_buffer) - 1, -1, -1):
                rgb_data = self.rgb_buffer[i]
                rgb_timestamp = rgb_data['timestamp']

                if rgb_timestamp < target_timestamp:
                    time_diff = target_timestamp - rgb_timestamp

                    if time_diff > self.max_time_diff:
                        # Found old frame, skip
                        break

                    if time_diff < min_time_diff:
                        min_time_diff = time_diff
                        best_rgb = rgb_data
                        best_rgb_index = i

                        if time_diff < 5:
                            break
                else:
                    # future rgb frame
                    continue

            if best_rgb and min_time_diff <= self.max_time_diff:
                # Success match
                # a Get success matched depth frame
                matched_depth = self.depth_buffer.pop()

                # b. Pop success matched rgb frame
                del self.rgb_buffer[best_rgb_index]

                # c. Clean up obsolete frame data
                rgb_ts_to_clear = best_rgb['timestamp']
                cleared_count = 0
                while self.rgb_buffer and self.rgb_buffer[0]['timestamp'] < rgb_ts_to_clear:
                    self.rgb_buffer.popleft()
                    cleared_count += 1

                # if cleared_count > 0:
                #     self.logger.log_to_file(f" ✅ [Success] Matched (Diff:{min_time_diff:.2f}ms). Cleared {cleared_count} stale RGB frames.")
                # else:
                #     self.logger.log_to_file(f" ✅ [Success] Matched (Diff:{min_time_diff:.2f}ms).")

                self.matched_count += 1

                # update
                self.depth_updated = False
                return best_rgb, matched_depth
            else:
                # Failed match
                if len(self.depth_buffer) > 0:
                    oldest_depth_ts = self.depth_buffer[0]['timestamp']
                    if target_timestamp - oldest_depth_ts > self.max_time_diff * 4:
                        self.depth_buffer.popleft()
                        # self.logger.log_to_file(f" ⚠️ [Warn] Discarded one stale Depth frame (TS:{oldest_depth_ts}).")

                # self.logger.log_to_file(f" 🚨 [Error] Failed to find matched RGB for latest Depth (TS:{target_timestamp}). Min Diff: {min_time_diff if min_time_diff != float('inf') else 'inf'} ms.")
                # update
                self.depth_updated = False
                return None, None



    def clear(self):
        """清空缓冲区"""
        with self.lock:
            self.rgb_buffer.clear()
            self.depth_buffer.clear()

    def get_stats(self):
        """获取统计信息"""
        with self.lock:
            return {
                'matched': self.matched_count,
                'rgb_buffered': len(self.rgb_buffer),
                'depth_buffered': len(self.depth_buffer),
                'rgb_dropped': self.rgb_dropped,
                'depth_dropped': self.depth_dropped
            }

""" ------------------------------------------ [AllStream管理器] ------------------------------------------ """
class StreamManager:
    def __init__(self, uvc_configs: dict, en_rgb_pointcloud: bool = True):
        self.logger = init_logger(RMSL_LOG_PATH)
        self.uvc_configs = uvc_configs
        self.en_rgb_pointcloud = en_rgb_pointcloud

        """ ==================================== Queue ==================================== """
        self.raw_rgb_queue = BoundedQueue(maxsize=5, name="raw_rgb")
        self.raw_depth_queue = BoundedQueue(maxsize=5, name="raw_depth")
        self.raw_ir_queue = BoundedQueue(maxsize=2, name="raw_ir")

        """ ==================================== CaptureThread ==================================== """
        self.rgb_thread: Optional[FFmpegCaptureThread] = None
        self.depth_thread: Optional[FFmpegCaptureThread] = None
        self.ir_thread: Optional[FFmpegCaptureThread] = None

        self.stream_started = False
        """ ==================================== FrameMatchThread ==================================== """
        # 时间戳匹配队列（处理线程 -> 主线程维护）
        self.matcher = SensorDataMatcher(max_time_diff=200, max_queue_size=20)

        # UI显示缓存（处理线程 -> UI读取）
        self.ui_rgb = None
        self.ui_depth = None
        self.ui_depth_color = None
        self.ui_pointcloud = None
        self.ui_ir_left = None
        self.ui_ir_right = None
        self.ui_disp_diff = None
        self.ui_lock = Lock()

        # Save packet
        self.save_data = MatchedDataPack(0)

        # 配置
        self.running = False
        self.mode_3d = False
        self.debug_mode = False

        self.allocated_buffer = False

        """ ==================================== Camera UVC Config ==================================== """
        # TODO: if enable debug mode, rgb stream resolution change to: 1080x1280
        self.stream_configs = {
            'rgb': StreamConfig(
                uvc_name=self.uvc_configs['rgb']['name'],
                width=self.uvc_configs['rgb']['width'],
                height=self.uvc_configs['rgb']['height'],
                fps=self.uvc_configs['rgb']['fps'],
                pixel_format=self.uvc_configs['rgb']['format'].lower()
            ),
            'depth': StreamConfig(
                uvc_name=self.uvc_configs['depth']['name'],
                width=self.uvc_configs['depth']['width'],
                height=self.uvc_configs['depth']['height'],
                fps=self.uvc_configs['depth']['fps'],
                pixel_format=self.uvc_configs['depth']['format'].lower()
            ),
            'ir': StreamConfig(
                uvc_name=self.uvc_configs['ir']['name'],
                width=self.uvc_configs['ir']['width'],
                height=self.uvc_configs['ir']['height'],
                fps=self.uvc_configs['ir']['fps'],
                pixel_format=self.uvc_configs['ir']['format'].lower()
            )
        }

        """ ==================================== Stream Enable ==================================== """
        self.stream_enabled = {
            'rgb': True,  # 默认开启
            'depth': True,  # 默认开启
            'ir': False  # 默认关闭，DEBUG模式开启
        }

        """ ==================================== Status Tracking ==================================== """
        # 统计信息
        self.stats = {
            'rgb_fps': 0.0,
            'depth_fps': 0.0,
            'ir_fps': 0.0,
            'rgb_drop_rate':0.0,
            'rgb_repeat_rate': 0,
            'depth_drop_rate':0.0,
            'depth_repeat_rate': 0.0,
            'ir_drop_rate':0.0,
            'ir_repeat_rate': 0,
            'process_rgb_fps': 0.0,
            'process_depth_fps': 0.0,
            'process_ir_fps': 0.0,
            'process_pointcloud_fps': 0.0,
            'matched_count': 0
        }
        self.stats_lock = Lock()

    def _preallocate_buffers(self):
        """预分配所有图像缓冲区"""
        rgb_cfg = self.stream_configs['rgb']
        depth_cfg = self.stream_configs['depth']
        ir_cfg = self.stream_configs['ir']

        # RGB缓冲区
        self.ui_rgb = np.zeros(
            (rgb_cfg.height, rgb_cfg.width, 3),
            dtype=np.uint8
        )

        # Depth缓冲区
        self.ui_depth = np.zeros(
            (depth_cfg.height, depth_cfg.width),
            dtype=np.uint16
        )
        self.ui_depth_color = np.zeros(
            (depth_cfg.height, depth_cfg.width, 3),
            dtype=np.uint8
        )

        # 点云缓冲区(实际points是稀疏动态，所以不预分配)
        self.ui_pointcloud = None

        # IR缓冲区（仅DEBUG模式使用）
        if self.debug_mode:
            self.ui_ir_left = np.zeros(
                (ir_cfg.height, ir_cfg.width, 3),
                dtype=np.uint8
            )
            self.ui_ir_right = np.zeros(
                (ir_cfg.height, ir_cfg.width, 3),
                dtype=np.uint8
            )
            self.ui_disp_diff = np.zeros(
                (ir_cfg.height, ir_cfg.width, 3),
                dtype=np.uint8
            )
        else:
            self.ui_ir_left = None
            self.ui_ir_right = None
            self.ui_disp_diff = None

    """ ==================================== Config API ==================================== """
    def set_debug_mode(self, enabled: bool):
        """设置DEBUG模式（会自动启用/禁用IR流）"""
        if self.running:
            self.logger.log_to_file(" ⚠️ [Warning] Cannot change debug mode while running, please restart RMSLViewer tool.")
            return

        self.debug_mode = enabled
        self.stream_enabled['ir'] = enabled
        self.logger.log_to_file(f" 🔧 [Config] Debug mode: {enabled}, IR stream: {enabled}")

        if enabled:
            # TODO: FIXED SIZE, 2025-11-17
            self.update_stream_config('rgb', width= 1080, height=1280)

        # ✅ 重新分配缓冲区
        self.logger.log_to_file(f" 🔧 [Config] Re-allocate buffers!")
        self._preallocate_buffers()

    def enable_stream(self, stream_type: str, enabled: bool):
        """单独控制流开关"""
        if stream_type in self.stream_enabled:
            self.stream_enabled[stream_type] = enabled
            self.logger.log_to_file(f" 🔧 [Config] Stream '{stream_type}': {enabled}")

    def update_stream_config(self, stream_type: str, **kwargs):
        """更新流配置"""
        if stream_type in self.stream_configs:
            config = self.stream_configs[stream_type]
            for key, value in kwargs.items():
                if hasattr(config, key):
                    setattr(config, key, value)
            self.logger.log_to_file(f" 🔧 [Config] Updated '{stream_type}' config: {kwargs}")

            # ✅ 重新分配缓冲区
            self.logger.log_to_file(f" 🔧 [Config] '{stream_type}' Resolution change, Re-allocate buffers!")
            self._preallocate_buffers()

    """ ==================================== ALL Stream Manage ==================================== """

    def start(self):
        # ORDER: RGB --> DEPTH --> IR(Optional)
        if self.running:
            self.logger.log_to_file(" ⚠️ [Warning] StreamManager already running")
            return False

        """ ==================================== PreAllocate Memory ==================================== """
        if not self.allocated_buffer:
            self._preallocate_buffers()
            self.allocated_buffer = False

        self.logger.log_to_file(" 🚀 [Info] Starting StreamManager...")
        self.running = True

        # reset queue
        self.raw_rgb_queue.reset()
        self.raw_depth_queue.reset()
        self.raw_ir_queue.reset()

        # flag
        success = True

        # 1. RGB
        if self.stream_enabled['rgb']:
            success &= self._start_stream('rgb')
            time.sleep(0.2)

        # 2. DEPTH
        if self.stream_enabled['depth']:
            success &= self._start_stream('depth')
            time.sleep(0.2)

        # 3. IR (仅DEBUG模式)
        if self.stream_enabled['ir']:
            success &= self._start_stream('ir')
            time.sleep(0.2)

        # update
        self.stream_started = True

        if not success:
            self.logger.log_to_file(" 💥 [Error] Failed to start all streams!")
            self.stop()
            return False

        self.logger.log_to_file(" ✅ [Info] StreamManager started successfully")
        return True

    def stop(self):
        """ Order: IR -> DEPTH -> RGB）"""
        if not self.running:
            return

        self.logger.log_to_file(" 🎉 [Info] Stopping StreamManager...")
        self.running = False
        self.stream_started = False

        # 按逆序停止流
        if self.ir_thread:
            self._stop_stream('ir')
            time.sleep(0.3)

        if self.depth_thread:
            self._stop_stream('depth')
            time.sleep(0.3)

        if self.rgb_thread:
            self._stop_stream('rgb')
            time.sleep(0.3)

        # 关闭队列
        self.raw_rgb_queue.close()
        self.raw_depth_queue.close()
        self.raw_ir_queue.close()

        self.logger.log_to_file(" ✅ [Info] StreamManager stopped")
        self._print_stats()

    def _start_stream(self, stream_type: str) -> bool:
        """启动单个流"""
        # ✅ 安全检查：如果旧线程还在运行，先停止
        if stream_type == 'rgb' and self.rgb_thread and self.rgb_thread.is_alive():
            self.logger.log_to_file(f" ⚠️ [Warning] {stream_type} thread still running, stopping first...")
            self._stop_stream(stream_type)
            time.sleep(0.5)  # 等待完全停止
        elif stream_type == 'depth' and self.depth_thread and self.depth_thread.is_alive():
            self._stop_stream(stream_type)
            time.sleep(0.5)
        elif stream_type == 'ir' and self.ir_thread and self.ir_thread.is_alive():
            self._stop_stream(stream_type)
            time.sleep(0.5)

        config = self.stream_configs[stream_type]
        queue_map = {
            'rgb': self.raw_rgb_queue,
            'depth': self.raw_depth_queue,
            'ir': self.raw_ir_queue
        }

        thread = FFmpegCaptureThread(
            stream_mgr=self,
            output_queue=queue_map[stream_type],
            uvc_cam_name=config.uvc_name,
            width=config.width,
            height=config.height,
            fps=config.fps,
            pixel_format=config.pixel_format
        )

        thread.start()

        # 等待启动完成
        if not thread.wait_until_started(timeout=10):
            self.logger.log_to_file(f" 💥 [Error] Failed to start {stream_type} stream")
            thread.stop()
            return False

        # 保存线程引用
        if stream_type == 'rgb':
            self.rgb_thread = thread
        elif stream_type == 'depth':
            self.depth_thread = thread
        elif stream_type == 'ir':
            self.ir_thread = thread

        return True

    def _stop_stream(self, stream_type: str):
        """停止单个流"""
        if stream_type == 'rgb':
            if self.rgb_thread:
                self.rgb_thread.stop(timeout=2)
                self.rgb_thread = None

        if stream_type == 'depth':
            if self.depth_thread:
                self.depth_thread.stop(timeout=2)
                self.depth_thread = None

        if stream_type == 'ir':
            if self.ir_thread:
                self.ir_thread.stop(timeout=2)
                self.ir_thread = None

    """ ==================================== UI DATA Interface ==================================== """

    def update_ui_data(self, **kwargs):
        """更新UI显示数据"""
        with self.ui_lock:
            for key, value in kwargs.items():
                attr_name = f'ui_{key}'
                if hasattr(self, attr_name):
                    buffer = getattr(self, attr_name)
                    # ✅ 点云直接赋值（不预分配）
                    if key == 'pointcloud':
                        setattr(self, attr_name, value)
                    # 其他数据使用预分配缓冲区
                    elif value is not None and buffer is not None:
                        try:
                            np.copyto(buffer, value)
                        except ValueError as e:
                            # 形状不匹配时回退到直接赋值
                            self.logger.log_to_file(
                                f" ⚠️ [Warning] Shape mismatch for {key}, "
                                f"falling back to direct assignment: {e}"
                            )
                            setattr(self, attr_name, value)
                    else:
                        setattr(self, attr_name, value)

    def get_ui_data(self) -> dict:
        """UI线程获取显示数据"""
        with self.ui_lock:
            return {
                'rgb': self.ui_rgb,
                'depth': self.ui_depth,
                'depth_color': self.ui_depth_color,
                'pointcloud': self.ui_pointcloud,
                'ir_left': self.ui_ir_left,
                'ir_right': self.ui_ir_right,
                'disp_diff': self.ui_disp_diff
            }

    """ ==================================== Stats Tracking ==================================== """
    def update_stats(self, key: str, value):
        """更新统计信息"""
        with self.stats_lock:
            if key in self.stats:
                self.stats[key] = value

    def get_stats(self) -> Dict[str, Any]:
        """获取统计信息"""
        with self.stats_lock:
            stats = self.stats.copy()

            # 添加队列统计
            stats['queues'] = {
                'rgb': self.raw_rgb_queue.get_stats(),
                'depth': self.raw_depth_queue.get_stats(),
                'ir': self.raw_ir_queue.get_stats()
            }

            # add matched count
            stats['matcher'] = self.matcher.get_stats()

            return stats

    def _print_stats(self):
        """打印统计信息"""
        stats = self.get_stats()
        self.logger.log_to_file("=" * 80)
        self.logger.log_to_file(" 📊 StreamManager Statistics:")
        self.logger.log_to_file(f"  [RGB  ] Capture FPS: {stats['rgb_fps']:.2f}, Post-Process FPS: {stats['process_rgb_fps']:.2f}")
        self.logger.log_to_file(f"  [DEPTH] Capture FPS: {stats['depth_fps']:.2f}, Post-Process FPS: {stats['process_depth_fps']:.2f}")
        self.logger.log_to_file(f"  [IR   ] Capture FPS: {stats['ir_fps']:.2f}")
        self.logger.log_to_file(f"  [PC   ] Post-Process FPS: {stats['process_pointcloud_fps']:.2f}")
        self.logger.log_to_file(f"  [RGB  ] Drop Rate: {stats['rgb_drop_rate']:.2f}%, Repeat Rate: {stats['rgb_repeat_rate']:.2f}%")
        self.logger.log_to_file(f"  [DEPTH] Drop Rate: {stats['depth_drop_rate']:.2f}%, Repeat Rate: {stats['depth_repeat_rate']:.2f}%")
        self.logger.log_to_file(f"  [IR   ] Drop Rate: {stats['ir_drop_rate']:.2f}%, Repeat Rate: {stats['ir_repeat_rate']:.2f}%")
        self.logger.log_to_file(f"  [DEPTH & RGB Matched Count]: {stats['matcher']['matched']}")

        self.logger.log_to_file("  " + "=" * 78)
        for name, queue_stats in stats['queues'].items():
            self.logger.log_to_file(f"  Queue '{name}': drops={queue_stats['total_drops']}, "
                                    f"rate={queue_stats['drop_rate']:.2%}")
        self.logger.log_to_file("=" * 80)


""" ------------------------------------------ [Capturer管理器] ------------------------------------------ """
class FFmpegCaptureThread(Thread):
    def __init__(self,
                 stream_mgr: StreamManager,
                 output_queue: BoundedQueue,
                 uvc_cam_name: str = 'RMSL321_RGB',
                 width: int = 1280,
                 height: int = 1080,
                 fps: int = 30,
                 pixel_format: str = 'yuyv422'):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self.output_queue = output_queue
        self.uvc_cam_name = uvc_cam_name
        self.stream_type = uvc_cam_name.split('_')[1].lower()
        self.width = width
        self.height = height
        self.fps = fps
        self.pixel_format = pixel_format

        self._stop_event = Event()  # 停止信号
        self._started_event = Event()  # 启动完成信号

        self.process = None
        self.pipe = None
        self.frame_count = 0
        self.frame_size = self._calculate_frame_size()

        # 预分配内存
        self.raw_buffer = bytearray(self.frame_size)
        self.raw_view = memoryview(self.raw_buffer)

        # process buffer
        self.yuyv_buffer = np.zeros((self.height, self.width), dtype=np.uint16)
        self.nv12_buffer = np.zeros((self.height * 3 // 2, self.width), dtype=np.uint8)
        self.rgb_buffer = np.zeros((self.height, self.width, 3), dtype=np.uint8)

        # Header
        self.header_size = 28
        self.header_struct = struct.Struct('<HHHHHH I Q I')

        self.ffmpeg_command = [
            FFMPEG,
            '-f', 'dshow',
            '-rtbufsize', '64M',
            '-video_size', f'{self.width}x{self.height}',
            '-framerate', str(fps),
            '-pixel_format', self.pixel_format,
            '-i', f'video={self.uvc_cam_name}',
            '-c:v', 'rawvideo',
            '-f', 'image2pipe',
            '-'
        ]

        if sys.platform == 'win32':
            self.creation_flags = subprocess.CREATE_NO_WINDOW  # 0x08000000
        else:
            self.creation_flags = 0

        # del duplicate
        self.repeat_cnt = 0
        self.last_ts = -1  # rgb depth
        self.last_type = -1  # ir

        # performance
        self.stat_interval = 30 # s
        self.fps_counter = 0
        self.fps_start_time = time.time()

        # 自适应丢帧检测
        self.learning_phase = True  # 学习阶段标志
        self.learning_samples = []  # 存储学习阶段的帧间隔
        self.learning_sample_size = 50  # 学习样本数量（可配置）

        # 统计值（学习完成后计算）
        self.avg_frame_interval = None  # 平均帧间隔
        self.std_frame_interval = None  # 标准差
        self.max_normal_interval = None  # 正常最大间隔

        # 丢帧检测
        self.dropped_frames_count = 0

    def _calculate_frame_size(self):
        format_sizes = {
            'yuyv422': self.width * self.height * 2,
            'bgr24': self.width * self.height * 3,
            'rgb24': self.width * self.height * 3,
            'nv12': self.width * self.height * 3 // 2,
        }
        return format_sizes.get(self.pixel_format, self.width * self.height * 2)

    def run(self):
        self.logger.log_to_file(
            f" 🎉 [Info] [{self.stream_type.upper()}] Starting stream: {self.uvc_cam_name}, [Config] width: {self.width}, height: {self.height}, fps: {self.fps}, pixel_format: {self.pixel_format}"
        )

        try:
            self.pipe = subprocess.Popen(
                self.ffmpeg_command,
                stdout=subprocess.PIPE,
                stderr=subprocess.DEVNULL,
                bufsize=self.frame_size,
                creationflags=self.creation_flags
            )

            time.sleep(0.2)
            if self.pipe.poll() is not None:
                stderr_output = self.pipe.stderr.read().decode()
                self.logger.log_to_file(
                    f" 💥 [Error] [FFmpegCaptureThread] [{self.stream_type.upper()}] FFmpeg failed to start: {stderr_output}"
                )
                return

            self._started_event.set()
            self.logger.log_to_file(
                f" ✅ [Info] [{self.stream_type.upper()}] Stream started successfully"
            )

            while not self._stop_event.is_set():
                bytes_read = self.pipe.stdout.readinto(self.raw_view)

                if bytes_read == self.frame_size:
                    self._process_and_queue_frame()
                elif bytes_read == 0:
                    if self.pipe.poll() is not None:
                        self.logger.log_to_file(
                            f" 💥 [Error] [FFmpegCaptureThread] [{self.stream_type.upper()}] FFmpeg process terminated"
                        )
                        break
                else:
                    self.logger.log_to_file(
                        f" ⚠️ [Warning] [FFmpegCaptureThread] [{self.stream_type.upper()}] Incomplete frame: {bytes_read} / {self.frame_size} bytes"
                    )
                    time.sleep(0.001)
        except FileNotFoundError:
            self.logger.log_to_file(
                f" 💥 [Error] [FFmpegCaptureThread] [{self.stream_type.upper()}] FFmpeg not found: {self.ffmpeg_command[0]}"
            )
        except Exception as e:
            self.logger.log_to_file(
                f" 💥 [Error] [FFmpegCaptureThread] [{self.stream_type.upper()}] Exception: {e}"
            )
        finally:
            self._cleanup()

    def _process_and_queue_frame(self):
        """处理并入队一帧数据"""
        # 解析Header
        header_bytes = bytes(self.raw_buffer[:self.header_size])
        try:
            _fields = self.header_struct.unpack(header_bytes)
        except struct.error as e:
            self.logger.log_to_file(f" 💥 [Error] [{self.stream_type.upper()}] Header unpack error: {e}")
            return

        _keys = ['magic', 'version', 'size', 'type', 'width', 'height',
                 'sequence', 'timestamp', 'reserve']
        result = dict(zip(_keys, _fields))

        # 验证Magic
        if result['magic'] != 0xA55A:
            return

        ts = int(result['timestamp'] / 1000) # us to ms
        frame_type = result['type']

        # 去重逻辑
        if ts < self.last_ts:
            self.logger.log_to_file(f" ⚠️ [Warn] [{self.stream_type.upper()}] Timestamp rollback: {self.last_ts}ms -> {ts}ms")
            return

        # 丢帧检测（在去重逻辑之前）
        if 0 < self.last_ts < ts:  # 确保有有效的间隔
            interval = ts - self.last_ts

            if self.learning_phase:
                self._learning_phase_update(interval)
            else:
                self._check_frame_drop(ts, self.last_ts, interval)

        if ts == self.last_ts:
            if frame_type in [1, 2] or frame_type == self.last_type:
                self.repeat_cnt += 1
                # self.logger.log_to_file(f" ⚠️ [Warn] [{self.stream_type.upper()}] ID: {result['sequence']}, Timestamp repeat: {self.last_ts}ms == {ts}ms")
                return

        self.last_ts = ts
        self.last_type = frame_type

        # 处理帧数据
        frame = self._process_frame(frame_type)
        if frame is None:
            return

        # 创建FrameData
        label = FFmpegCaptureThread._get_label(frame_type)
        frame_data = FrameData(
            timestamp=ts,
            frame_id=self.frame_count,
            label=label,
            data=frame
        )
        # update
        # ✅ 非阻塞 put，避免卡死
        if not self.output_queue.put(frame_data, drop_old=True):
            # 放入失败也不影响继续采集
            pass
        else:
            self.frame_count += 1

        # cal fps
        self._update_fps_stats()

    def _learning_phase_update(self, interval):
        if interval < 500:
            self.learning_samples.append(interval)

        if len(self.learning_samples) >= self.learning_sample_size:
            completion_reason = f"collected {len(self.learning_samples)} samples"
            self._complete_learning_phase(completion_reason)

    def _complete_learning_phase(self, reason):
        if not self.learning_samples:
            self.avg_frame_interval = self.fps  # 默认25fps
            self.std_frame_interval = 10
        else:
            # 计算统计值
            intervals = np.array(self.learning_samples)
            self.avg_frame_interval = np.mean(intervals)
            self.std_frame_interval = np.std(intervals)

            p95 = np.percentile(intervals, 95)
            p99 = np.percentile(intervals, 99)

            # 设置正常最大间隔（可以用p95或p99，或者mean+3*std）这里使用较为宽松的策略：max(p95, mean+3*std)
            self.max_normal_interval = max(p95, self.avg_frame_interval + 3 * self.std_frame_interval)
            self.max_normal_interval = min(self.max_normal_interval, self.avg_frame_interval * 3)

        self.learning_phase = False

    def _check_frame_drop(self, current_ts, last_ts, interval):
        """基于学习的统计值检测丢帧"""
        if self.avg_frame_interval is None:
            return

        # 判断是否超过正常范围
        if interval > self.max_normal_interval:
            # 估算丢帧数量
            estimated_dropped = round(interval / self.avg_frame_interval) - 1

            if estimated_dropped > 0:
                self.dropped_frames_count += estimated_dropped

    def _process_frame(self, frame_type):
        try:
            if frame_type == 2:
                # YUYV uint16 depth data
                data_view = np.frombuffer(
                    self.raw_buffer[:self.height * self.width * 2],
                    dtype=np.uint16
                ).reshape((self.height, self.width))

                # overlap header
                if data_view.shape[1] >= self.header_size:
                    data_view[0, :self.header_size] = data_view[1, :self.header_size]

                return data_view.copy()

            elif frame_type in [1, 4, 5]:
                # NV12 formats
                nv12_view = np.frombuffer(
                    self.raw_buffer,
                    dtype=np.uint8,
                    count=self.height * self.width * 3 // 2
                ).reshape(self.height * 3 // 2, self.width)

                rgb_frame = cv2.cvtColor(nv12_view, cv2.COLOR_YUV2RGB_NV12)

                # 修复header污染
                if rgb_frame.shape[1] >= self.header_size:
                    rgb_frame[0, :self.header_size] = rgb_frame[1, :self.header_size]

                return rgb_frame

            else:
                return None

        except Exception as e:
            self.logger.log_to_file(
                f" 💥 [Error] [{self.stream_type.upper()}] Frame processing error: {e}"
            )
            return None

    @staticmethod
    def _get_label(frame_type):
        labels = {1: 'rgb', 2: 'depth', 4: 'ir_l', 5: 'ir_r'}
        return labels.get(frame_type, 'unknown')

    def _update_fps_stats(self):
        """更新FPS统计"""
        self.fps_counter += 1
        current_time = time.time()
        elapsed = current_time - self.fps_start_time

        if elapsed >= self.stat_interval:
            fps = self.fps_counter / elapsed
            self.stream_mgr.update_stats(f'{self.stream_type}_fps', fps)

            if not self.learning_phase and self.dropped_frames_count > 0:
                total_frames = self.frame_count + self.dropped_frames_count
                drop_rate = (self.dropped_frames_count / total_frames * 100) if total_frames > 0 else 0
                # actual_fps = 1000.0 / self.avg_frame_interval if self.avg_frame_interval else 0

                # self.logger.log_to_file(
                #     f" 📊 [{self.stream_type.upper()}] Frame drop stats in last {elapsed:.1f}s, Estimate FPS: {actual_fps:.1f}, : "
                #     f"Received: {self.frame_count}, "
                #     f"Dropped: {self.dropped_frames_count}, "
                #     f"Drop rate: {drop_rate:.2f}%"
                # )
                self.stream_mgr.update_stats(f'{self.stream_type}_drop_rate', drop_rate)
            self.stream_mgr.update_stats(f'{self.stream_type}_repeat_rate', (self.repeat_cnt / self.frame_count) * 100)

            self.fps_counter = 0
            self.fps_start_time = current_time

    def stop(self, timeout=5):
        """停止线程"""
        if not self.is_alive():
            return

        self.logger.log_to_file(
            f" 🎉 [Info] [{self.stream_type.upper()}] Stopping stream..."
        )

        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [{self.stream_type.upper()}] Thread did not stop gracefully"
            )
            # 强制清理
            self._cleanup()

    def wait_until_started(self, timeout=10):
        return self._started_event.wait(timeout=timeout)

    def _cleanup(self):
        """清理资源"""
        self.logger.log_to_file(
            f" 🧹 [Info] [{self.stream_type.upper()}] Cleaning up..."
        )

        try:
            if self.pipe:
                if self.pipe.poll() is None:
                    self.pipe.terminate()
                    try:
                        self.pipe.wait(timeout=3)
                    except subprocess.TimeoutExpired:
                        self.pipe.kill()
                        self.pipe.wait()

                # ✅ 关键：关闭所有文件句柄
                try:
                    if self.pipe.stdout:
                        self.pipe.stdout.close()
                    if self.pipe.stderr:
                        self.pipe.stderr.close()
                    if self.pipe.stdin:
                        self.pipe.stdin.close()
                except Exception as e:
                    self.logger.log_to_file(
                        f" ⚠️ [Warning] [{self.stream_type.upper()}] Error closing pipes: {e}"
                    )

                self.pipe = None
        except Exception as e:
            self.logger.log_to_file(
                f" ⚠️ [Warning] [{self.stream_type.upper()}] Cleanup error: {e}"
            )

        self.logger.log_to_file(
            f" ✅ [Info] [{self.stream_type.upper()}] Stopped. Frames processed: {self.frame_count}"
        )


class ProcessRgbThread(Thread):
    def __init__(self, stream_mgr: StreamManager, calib_info: dict, config_info: dict, en_undistort: bool = False):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self._stop_event = Event()

        self.en_undistort = en_undistort
        self.calib_info = calib_info
        self.config_info = config_info
        self.module_category = self.calib_info.get('category', 'rgbd')

        # Get current rgb frame size
        self.rgb_w = self.stream_mgr.stream_configs['rgb'].width
        self.rgb_h = self.stream_mgr.stream_configs['rgb'].height

        # Rectify initial
        if self.en_undistort:
            self._initial_rectify()

        # fps tracking
        self.fps_counter = deque(maxlen=50)

        # ✅ 深度数据缓冲区
        # self.rgb_buffer = np.zeros((self.rgb_h, self.rgb_w, 3), dtype=np.uint16)

    def _initial_rectify(self):
        if self.module_category == 'rgbd':
            new_cam_matrix, _ = cv2.getOptimalNewCameraMatrix(
                self.calib_info['rgb_intrinsic']['K'],
                self.calib_info['rgb_distortion']['D'],
                (self.calib_info['rgb_width'], self.calib_info['rgb_height']),
                alpha=0,
                newImgSize=(self.calib_info['rgb_width'], self.calib_info['rgb_height'])
            )

            self.mapx, self.mapy = cv2.initUndistortRectifyMap(
                self.calib_info['rgb_intrinsic']['K'],
                self.calib_info['rgb_distortion']['D'],
                None,
                new_cam_matrix,
                (self.calib_info['rgb_width'], self.calib_info['rgb_height']),
                cv2.CV_32FC1
            )
        elif self.module_category == '2rgb':
            self.mapx, self.mapy = cv2.initUndistortRectifyMap(
                self.calib_info['l_intrinsic']['K_l'],
                self.calib_info['l_distortion']['D_l'],
                self.calib_info['R1'],
                self.calib_info['P1'],
                (self.calib_info['l_width'], self.calib_info['l_height']),
                cv2.CV_32FC1
            )
        else:
            self.logger.log_to_file(f" 🚨 [Error] Rectify failed initialization, not support module category: {self.module_category}.")
            raise ValueError(f" 🚨 [Error] Rectify failed initialization, not support module category: {self.module_category}.")

        self.logger.log_to_file(f" 🎉 [Info] Successfully initialized rgb rectify map params.")

    def _rectify_rgb(self, ori_img: np.ndarray) -> np.ndarray:
        if self.module_category == '2rgb':
            rgb_w = self.calib_info['l_width']
            rgb_h = self.calib_info['l_height']

            img_rgb = ori_img
            if self.rgb_w == rgb_w * 2:
                img_rgb = ori_img[::2, ::2]
                crop_total = int(self.rgb_h / 2 - rgb_h)
                crop_top = crop_total // 2
                crop_bottom = crop_total - crop_top
                img_rgb = img_rgb[crop_top: int(self.rgb_h / 2) - crop_bottom, :]
        else:
            img_rgb = ori_img

        return cv2.remap(img_rgb, self.mapx, self.mapy, cv2.INTER_LINEAR)

    def run(self):
        while not self._stop_event.is_set():
            if not self.stream_mgr.stream_started:
                time.sleep(0.02)
                continue

            rgb_data = self.stream_mgr.raw_rgb_queue.get_latest(timeout=0.05)
            if rgb_data is None:
                time.sleep(0.02)
                continue

            if self.en_undistort:
                rgb_frame = self._rectify_rgb(ori_img=rgb_data.data)
            else:
                rgb_frame = rgb_data.data

            ts = rgb_data.timestamp

            # add to match queue
            self.stream_mgr.matcher.add_rgb_data(rgb_frame, ts)

            # update ui show
            self.stream_mgr.update_ui_data(rgb=rgb_frame)

            # stat process fps of rgb
            self.fps_counter.append(time.time())
            if len(self.fps_counter) >= 25:
                fps = len(self.fps_counter) / (self.fps_counter[-1] - self.fps_counter[0])
                with self.stream_mgr.stats_lock:
                    self.stream_mgr.stats['process_rgb_fps'] = fps
        self.logger.log_to_file(f" 🎉 [Info] [ProcessRgbThread] exit.")

    def stop(self, timeout=5):
        """停止线程"""
        self.logger.log_to_file(
            f" 🎉 [Info] [ProcessRgbThread] Stopping stream..."
        )
        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [ProcessRgbThread] Thread did not stop gracefully"
            )


class ProcessDispThread(Thread):
    def __init__(self, stream_mgr: StreamManager, calib_info: dict, config_info: dict):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self._stop_event = Event()

        # get size
        self.disp_w = self.stream_mgr.stream_configs['depth'].width
        self.disp_h = self.stream_mgr.stream_configs['depth'].height
        self.calib_disp_w = calib_info['l_width']
        self.calib_disp_h = calib_info['l_height']

        scale = self.disp_w / self.calib_disp_w

        # Calibration info
        self.calib_info = calib_info
        self.q23 = self.calib_info['q23']
        if scale != 1.0:
            self.q23 = self.q23 / scale

        self.q32 = self.calib_info['q32']
        self.q33 = self.calib_info['q33']
        self.module_category = self.calib_info.get('category', 'rgbd')

        # Config info
        self.en_debug = config_info['en_debug']
        self.min_depth_dis = config_info.get('min_distance_mm', 100)  # mm
        self.max_depth_dis = config_info.get('max_distance_mm', 5000)  # mm
        self.colorizer_mode = config_info.get('colorize_mode', 'jet_red2blue')

        #  api
        self.utils_pc = UtilsPointCloud()
        self.colorizer = Colorizer(lut_size=256, mode=self.colorizer_mode)

        # fps tracking
        self.fps_counter = deque(maxlen=50)

        # ✅ 预分配处理缓冲区
        self._preallocate_buffers()

        # flag
        self.last_disp = None

    def _preallocate_buffers(self):
        """预分配所有处理缓冲区"""
        shape = (self.disp_h, self.disp_w)

        # ✅ 深度数据缓冲区
        self.depth_buffer = np.zeros(shape, dtype=np.uint16)

        # ✅ 深度彩色图缓冲区
        self.depth_color_buffer = np.zeros((*shape, 3), dtype=np.uint8)

        # ✅ 差分图缓冲区（debug模式）
        if self.en_debug:
            self.disp_diff_buffer = np.zeros(shape, dtype=np.int16)  # 注意用int16存储差分
            self.last_disp_buffer = np.zeros(shape, dtype=np.uint16)
            self.disp_diff_color_buffer = np.zeros((*shape, 3), dtype=np.uint8)
        else:
            self.disp_diff_buffer = None
            self.last_disp_buffer = None

    def run(self):
        while not self._stop_event.is_set():
            if not self.stream_mgr.stream_started:
                time.sleep(0.02)
                continue

            disp_data = self.stream_mgr.raw_depth_queue.get_latest(timeout=0.05)
            if disp_data is None:
                time.sleep(0.02)
                continue

            disp_frame = disp_data.data
            ts = disp_data.timestamp

            # cal pse-depth
            depth = self.utils_pc.disp_to_depth(
                disp_frame,
                self.q23, self.q32, self.q33,
                subpixel_value=64,
                zoom_ratio=1.0,
                out=self.depth_buffer  # 👈 传入预分配缓冲区
            )
            if depth is None:
                continue

            _ = self.colorizer.colorize(depth, depth_range=(self.min_depth_dis, self.max_depth_dis), out=self.depth_color_buffer)

            # add to match queue
            self.stream_mgr.matcher.add_depth_data(disp_frame, self.depth_buffer.copy(), ts, self.depth_color_buffer.copy())

            # update ui show
            self.stream_mgr.update_ui_data(depth=self.depth_buffer, depth_color=self.depth_color_buffer)

            # ✅ Debug模式：使用预分配缓冲区计算差分
            if self.en_debug:
                if self.last_disp is not None:
                    # 使用numpy的out参数避免临时数组
                    np.subtract(disp_frame, self.last_disp_buffer,
                                out=self.disp_diff_buffer,
                                casting='unsafe')
                    _ = self.colorizer.colorize(self.disp_diff_buffer, depth_range=(5, 500), out=self.disp_diff_color_buffer)
                    self.stream_mgr.update_ui_data(disp_diff=self.disp_diff_color_buffer)

                # 保存当前帧到last_disp_buffer
                np.copyto(self.last_disp_buffer, disp_frame)
                self.last_disp = True  # 标志位

            # stat process fps of rgb
            self.fps_counter.append(time.time())
            if len(self.fps_counter) >= 25:
                fps = len(self.fps_counter) / (self.fps_counter[-1] - self.fps_counter[0])
                with self.stream_mgr.stats_lock:
                    self.stream_mgr.stats['process_depth_fps'] = fps
        self.logger.log_to_file(f" 🎉 [Info] [ProcessDispThread] exit.")

    def stop(self, timeout=5):
        """停止线程"""
        self.logger.log_to_file(
            f" 🎉 [Info] [ProcessDispThread] Stopping stream..."
        )
        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [ProcessDispThread] Thread did not stop gracefully"
            )


class ProcessPointCloudThread(Thread):
    def __init__(self, stream_mgr: StreamManager, calib_info: dict, config_info: dict):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self._stop_event = Event()

        self.calib_info = calib_info
        self.utils_pc = UtilsPointCloud()
        self.en_full_pc = False

        self.min_depth_dis = config_info.get('min_distance_mm', 100)  # mm
        self.max_depth_dis = config_info.get('max_distance_mm', 5000)  # mm
        self.save_rgb_pointcloud = config_info.get('save_rgb_pointcloud', 0)

        # calib file, image size
        self.calib_rgb_w = self.calib_info.get('rgb_width', 1280)
        self.calib_rgb_h = self.calib_info.get('rgb_height', 1080)
        self.calib_disp_w = self.calib_info.get('l_width', 640)
        self.calib_disp_h = self.calib_info.get('l_height', 352)

        # uvc enable config
        self.rgb_w = self.stream_mgr.stream_configs['rgb'].width
        self.rgb_h = self.stream_mgr.stream_configs['rgb'].height
        self.disp_w = self.stream_mgr.stream_configs['depth'].width
        self.disp_h = self.stream_mgr.stream_configs['depth'].height

        scale_rgb = self.rgb_w / self.calib_rgb_w
        scale_disp = self.disp_w / self.calib_disp_w

        # Get intrinsic params
        self.fx = self.calib_info['l_intrinsic']['fx'] * scale_disp
        self.fy = self.calib_info['l_intrinsic']['fy'] * scale_disp
        self.cx = self.calib_info['l_intrinsic']['cx'] * scale_disp
        self.cy = self.calib_info['l_intrinsic']['cy'] * scale_disp

        self.module_category = self.calib_info['category']

        if self.save_rgb_pointcloud == 1 and self.module_category == 'rgbd':
            # Get r_coeff and t_coeff
            self.r_coeff = {k: v * scale_rgb for k, v in self.calib_info['r_coeff'].items()}
            self.t_coeff = {k: v * scale_rgb for k, v in self.calib_info['t_coeff'].items()}

            self.aligner = RMSLAlign(src_w=self.disp_w,
                                     src_h=self.disp_h,
                                     dst_w=self.rgb_w,
                                     dst_h=self.rgb_h,
                                     extend_w_pixels=256,
                                     extend_h_pixels=256,
                                     rotate_angle=0)
            self.aligner.create_lut(self.r_coeff)

        # fps tracking
        self.fps_counter = deque(maxlen=50)

    def en_full_pointcloud_calculate(self, enabled: bool):
        self.en_full_pc = enabled

    def run(self):
        while not self._stop_event.is_set():
            if not self.stream_mgr.stream_started:
                time.sleep(0.02)
                continue

            if not self.stream_mgr.mode_3d:
                time.sleep(0.1)
                continue

            matched_rgb_data, matched_depth_data = self.stream_mgr.matcher.find_best_match()
            if matched_rgb_data is None or matched_depth_data is None:
                time.sleep(0.05)
                continue

            matched_rgb = matched_rgb_data['img']
            ts_rgb = matched_rgb_data['timestamp']
            matched_depth = matched_depth_data['depth']
            matched_disp = matched_depth_data['disp']
            matched_depth_color = matched_depth_data['pse_color']
            ts_depth = matched_depth_data['timestamp']

            factor = 1
            if self.rgb_w > 640:
                factor = 8

            if self.save_rgb_pointcloud == 1:
                # 保存rgb点云
                if self.module_category == 'rgbd':
                    # rgbd
                    align_depth_img = self.aligner.align_dp_to_rgb(matched_depth, self.t_coeff)
                    pointcloud = self.utils_pc.generate_rgb_pointcloud(matched_rgb,
                                                                       align_depth_img,
                                                                       self.calib_info['rgb_intrinsic'],
                                                                       near_clip_mm=self.min_depth_dis,
                                                                       far_clip_mm=self.max_depth_dis,
                                                                       downsample_factor=factor,
                                                                       convert_unit_m=True)
                else:
                    # 2rgb
                    pointcloud = self.utils_pc.generate_rgb_pointcloud(matched_rgb,
                                                                       matched_depth,
                                                                       self.calib_info['l_intrinsic'],
                                                                       near_clip_mm=self.min_depth_dis,
                                                                       far_clip_mm=self.max_depth_dis,
                                                                       downsample_factor=factor,
                                                                       convert_unit_m=True)
            else:
                pointcloud, _ = self.utils_pc.generate_pointcloud(matched_depth,
                                                                  self.fx, self.fy, self.cx, self.cy,
                                                                  matched_depth.shape[0],
                                                                  matched_depth.shape[1],
                                                                  self.min_depth_dis,
                                                                  self.max_depth_dis,
                                                                  downsample_factor=factor,
                                                                  convert_unit_m=True,
                                                                  en_valid_roi=False,
                                                                  en_cen_roi=False)

            # update ui show
            self.stream_mgr.update_ui_data(pointcloud=pointcloud)

            # Save data
            self.stream_mgr.save_data.timestamp = ts_depth
            self.stream_mgr.save_data.depth = matched_depth
            self.stream_mgr.save_data.disp = matched_disp
            self.stream_mgr.save_data.rgb = matched_rgb
            self.stream_mgr.save_data.depth_color = matched_depth_color
            self.stream_mgr.save_data.time_diff = ts_depth - ts_rgb

            if self.en_full_pc:
                if self.save_rgb_pointcloud == 1:
                    # 保存rgb点云
                    if self.module_category == 'rgbd':
                        # rgbd
                        align_depth_img = self.aligner.align_dp_to_rgb(matched_depth, self.t_coeff)
                        pointcloud_full = self.utils_pc.generate_rgb_pointcloud(matched_rgb,
                                                                                align_depth_img,
                                                                                self.calib_info['rgb_intrinsic'],
                                                                                near_clip_mm=self.min_depth_dis,
                                                                                far_clip_mm=self.max_depth_dis,
                                                                                downsample_factor=1,
                                                                                convert_unit_m=True)
                    else:
                        # 2rgb
                        pointcloud_full = self.utils_pc.generate_rgb_pointcloud(matched_rgb,
                                                                                matched_depth,
                                                                                self.calib_info['l_intrinsic'],
                                                                                near_clip_mm=self.min_depth_dis,
                                                                                far_clip_mm=self.max_depth_dis,
                                                                                downsample_factor=1,
                                                                                convert_unit_m=True)
                else:
                    pointcloud_full, _ = self.utils_pc.generate_pointcloud(matched_depth,
                                                                           self.fx, self.fy, self.cx, self.cy,
                                                                           matched_depth.shape[0],
                                                                           matched_depth.shape[1],
                                                                           self.min_depth_dis,
                                                                           self.max_depth_dis,
                                                                           downsample_factor=1,
                                                                           convert_unit_m=True,
                                                                           en_valid_roi=False,
                                                                           en_cen_roi=False)

                self.stream_mgr.save_data.pointcloud = pointcloud_full
                # reset
                self.en_full_pc = False

            # stat process fps of rgb
            self.fps_counter.append(time.time())
            if len(self.fps_counter) >= 25:
                fps = len(self.fps_counter) / (self.fps_counter[-1] - self.fps_counter[0])
                with self.stream_mgr.stats_lock:
                    self.stream_mgr.stats['process_pointcloud_fps'] = fps
        self.logger.log_to_file(f" 🎉 [Info] [ProcessPointCloudThread] exit.")

    def stop(self, timeout=5):
        """停止线程"""
        self.logger.log_to_file(
            f" 🎉 [Info] [ProcessPointCloudThread] Stopping stream..."
        )
        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [ProcessPointCloudThread] Thread did not stop gracefully"
            )


class ProcessIRThread(Thread):
    def __init__(self, stream_mgr: StreamManager):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self._stop_event = Event()

        # fps tracking
        self.fps_counter = deque(maxlen=50)

    def run(self):
        while not self._stop_event.is_set():
            if not self.stream_mgr.stream_started:
                time.sleep(0.02)
                continue

            if not self.stream_mgr.debug_mode:
                time.sleep(0.02)
                continue

            data_a = self.stream_mgr.raw_ir_queue.get(timeout=0.05)
            data_b = self.stream_mgr.raw_ir_queue.get(timeout=0.05)
            if data_a is None or data_b is None:
                time.sleep(0.02)
                continue
            ir_l, ir_r = data_a.data, data_b.data
            if data_a.label == 'ir_r' and data_b.label == 'ir_l':
                ir_l, ir_r = ir_r, ir_l

            self.stream_mgr.update_ui_data(
                ir_left=ir_l,
                ir_right=ir_r
            )

            self.stream_mgr.save_data.ir_left = ir_l
            self.stream_mgr.save_data.ir_right = ir_r

            # stat process fps of rgb
            self.fps_counter.append(time.time())
            if len(self.fps_counter) >= 25:
                fps = len(self.fps_counter) / (self.fps_counter[-1] - self.fps_counter[0])
                with self.stream_mgr.stats_lock:
                    self.stream_mgr.stats['process_ir_fps'] = fps
        self.logger.log_to_file(f" 🎉 [Info] [ProcessIRThread] exit.")

    def stop(self, timeout=5):
        """停止线程"""
        self.logger.log_to_file(
            f" 🎉 [Info] [ProcessIRThread] Stopping stream..."
        )
        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [ProcessIRThread] Thread did not stop gracefully"
            )


class SaveDataThread(Thread):
    def __init__(self, stream_mgr: StreamManager, save_dir: str, config_info: dict):
        super().__init__(daemon=True)
        self.logger = init_logger(RMSL_LOG_PATH)
        self.stream_mgr = stream_mgr
        self._stop_event = Event()

        self.utils = UtilsFile()
        self.utils_pc = UtilsPointCloud()

        self.save_dir = save_dir
        self.utils.save_path_check(self.save_dir)
        self.en_save = False
        self.save_rgb_pointcloud = config_info.get('save_rgb_pointcloud', 0)

    def set_save_flag(self, en_save: bool):
        self.en_save = en_save

    @staticmethod
    def get_timestamp():
        formatted_time = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")

        return formatted_time

    def run(self):
        while not self._stop_event.is_set():
            if not self.stream_mgr.stream_started:
                time.sleep(0.02)
                continue

            if not self.en_save:
                time.sleep(0.1)
                continue

            all_data = self.stream_mgr.save_data
            if all_data.rgb is None or all_data.depth is None:
                matched_rgb_data, matched_depth_data = self.stream_mgr.matcher.find_best_match()
                if matched_rgb_data is None or matched_depth_data is None:
                    time.sleep(0.05)
                    continue

                self.stream_mgr.save_data.timestamp = matched_depth_data['timestamp']
                self.stream_mgr.save_data.depth = matched_depth_data['depth']
                self.stream_mgr.save_data.disp = matched_depth_data['disp']
                self.stream_mgr.save_data.rgb = matched_rgb_data['img']
                self.stream_mgr.save_data.depth_color = matched_depth_data['pse_color']
                self.stream_mgr.save_data.time_diff = matched_depth_data['timestamp'] - matched_rgb_data['timestamp']

            cur_ts = SaveDataThread.get_timestamp()

            if all_data.rgb is not None:
                self.utils.save_image(all_data.rgb, self.save_dir, prefix='rgb', timestamp=cur_ts, en_rgb2bgr=True)

            if all_data.depth is not None:
                self.utils.save_image(all_data.depth, self.save_dir, prefix='depth', timestamp=cur_ts, en_rgb2bgr=False)

            if all_data.disp is not None:
                self.utils.save_image(all_data.disp, self.save_dir, prefix='disp', timestamp=cur_ts, en_rgb2bgr=False)

            if all_data.depth_color is not None:
                self.utils.save_image(all_data.depth_color, self.save_dir, prefix='depth_color', timestamp=cur_ts, en_rgb2bgr=True)

            if self.stream_mgr.debug_mode:
                # save ir image
                if all_data.ir_left is not None:
                    self.utils.save_image(all_data.ir_left, self.save_dir, prefix='ir_left', timestamp=cur_ts, en_rgb2bgr=True)

                if all_data.ir_right is not None:
                    self.utils.save_image(all_data.ir_right, self.save_dir, prefix='ir_right', timestamp=cur_ts, en_rgb2bgr=True)

            # save pointcloud
            pointcloud_name = f'pointcloud_{cur_ts}.ply'
            pointcloud_path = os.path.join(self.save_dir, pointcloud_name)

            if all_data.pointcloud is not None:
                if self.save_rgb_pointcloud == 1:
                    self.utils_pc.save_ply_binary_rgb(all_data.pointcloud, pointcloud_path)
                else:
                    self.utils_pc.save_pointcloud_fast(all_data.pointcloud, pointcloud_path)

            self.en_save = False
        self.logger.log_to_file(f" 🎉 [Info] [SaveDataThread] exit.")

    def stop(self, timeout=5):
        """停止线程"""
        self.logger.log_to_file(
            f" 🎉 [Info] [SaveDataThread] Stopping stream..."
        )
        self._stop_event.set()

        # 等待线程结束
        self.join(timeout=timeout)

        if self.is_alive():
            self.logger.log_to_file(
                f" ⚠️ [Warning] [SaveDataThread] Thread did not stop gracefully"
            )


class CustomGLViewWidget(GLViewWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.mouse_sensitivity = 0.5  # 降低鼠标灵敏度（默认是1.0）
        self.pan_speed = 0.1  # 右键拖动速度
        self.mousePos = QtCore.QPoint()  # 初始化鼠标位置

    def mousePressEvent(self, ev):
        # 记录鼠标按下时的位置
        self.mousePos = ev.pos()

    def mouseMoveEvent(self, ev):
        if not hasattr(self, 'mousePos'):
            self.mousePos = ev.pos()
            return

        diff = ev.pos() - self.mousePos
        self.mousePos = ev.pos()

        if ev.buttons() == QtCore.Qt.LeftButton:
            # 左键旋转，应用灵敏度系数
            self.orbit(-diff.x() * self.mouse_sensitivity,
                       diff.y() * self.mouse_sensitivity)
        elif ev.buttons() == QtCore.Qt.MiddleButton:
            # 中键缩放
            if diff.y() != 0:
                scale_factor = 1.0 + diff.y() * 0.01 * self.mouse_sensitivity
                self.opts['distance'] *= scale_factor
                self.update()
        elif ev.buttons() == QtCore.Qt.RightButton:
            # 右键平移
            self.pan(diff.x() * self.pan_speed,
                     diff.y() * self.pan_speed, 0, relative='view')

    def wheelEvent(self, ev):
        # 调整滚轮缩放灵敏度
        delta = ev.angleDelta().y()
        if delta > 0:
            scale_factor = 1 - (0.1 * self.mouse_sensitivity)
        else:
            scale_factor = 1 + (0.1 * self.mouse_sensitivity)

        self.opts['distance'] *= scale_factor
        self.update()


class DeviceInitWorker(QThread):
    finished_signal = pyqtSignal(dict, dict, dict)
    progress_signal = pyqtSignal(int)  # 进度条数值 (0-100)
    status_signal = pyqtSignal(str)  # 进度条上的文字提示

    def __init__(self, controller: RMSLUVCController):
        super().__init__()
        self.controller = controller
        self.logger = init_logger(RMSL_LOG_PATH)

    def run(self):
        result_data = {
            'uvc_config': {'rgb': [], 'depth': [], 'ir': []},
            'used_uvc_config': {'rgb':{}, 'depth': {}, 'ir': {}},
            'calib_info': {},
            'cam_names': {}
        }

        try:
            self.progress_signal.emit(5)
            self.status_signal.emit("Initializing device controller...")

            self.controller = RMSLUVCController()
            devices = self.controller.enumerate_devices()

            total_devices = len(devices)
            self.status_signal.emit(f"Found {total_devices} device，enumeratoring...")

            for i, (idx, name) in enumerate(devices):
                # cal progress
                current_progress = 10 + int((i / total_devices) * 50)
                self.progress_signal.emit(current_progress)

                enumer = UVCFormatEnumerator(str(name))
                formats = enumer.get_supported_formats()

                sorted_formats = sorted(
                    formats,
                    key=lambda x: (x['width'], x['height']),
                    reverse=True
                )

                if 'rgb' in name.lower():
                    for config in sorted_formats: config['name'] = name
                    result_data['uvc_config']['rgb'] = sorted_formats
                    result_data['used_uvc_config']['rgb'] = sorted_formats[0]
                    result_data['cam_names']['rgb'] = name

                elif 'depth' in name.lower() or 'disp' in name.lower():
                    for config in sorted_formats: config['name'] = name
                    result_data['uvc_config']['depth'] = sorted_formats
                    result_data['used_uvc_config']['depth'] = sorted_formats[0]
                    result_data['cam_names']['depth'] = name

                elif 'ir' in name.lower():
                    for config in sorted_formats: config['name'] = name
                    result_data['uvc_config']['ir'] = sorted_formats
                    result_data['used_uvc_config']['ir'] = sorted_formats[0]
                    result_data['cam_names']['ir'] = name

            self.progress_signal.emit(60)
            self.status_signal.emit("Reading calibration file...")

            self.controller.init_device(0)
            calib_params = self.controller.get_calibration_data(verify_checksum=True)
            parser = CalibrationParser(calib_params.params)
            result_data['calib_info'] = parser.get_all_camera_intrinsic()

            self.progress_signal.emit(100)
            self.status_signal.emit("Finished initialization.")

            self.finished_signal.emit(result_data['uvc_config'], result_data['used_uvc_config'], result_data['calib_info'])

        except Exception as e:
            self.logger.log_to_file(f" 💥 [Error] Error in thread: {e}")


class RMSLViewer(QWidget, Ui_RMSLViewer):
    def __init__(self):
        super(RMSLViewer, self).__init__()
        self.logger = init_logger(RMSL_LOG_PATH)
        self.save_dir = str(PROJECT_ROOT / 'save')

        self.utils = UtilsFile()
        self.utils_pc = UtilsPointCloud()

        self.config_info = {
            "min_distance_mm": 100,
            "max_distance_mm": 3000,
            "colorize_mode": "jet_red2blue",
            "save_rgb_pointcloud": 1,
            "en_debug": False,
            "pointcloud_format": "ply",
            "device_current_ma": 1000,
            "algorithm_iterations": 3,
            "firmware_path": "",
            "en_plane_acc_cal": False,
            "acc_stacked_frame_cnt": 30,
            "acc_roi_w_scale": 0.9,
            "acc_roi_h_scale": 0.9,
            "acc_spec_roi_w": 24,
            "acc_spec_roi_h": 24
        }

        self.stream_mgr = None
        self.en_debug = False
        self.used_uvc_config = {}
        self.uvc_config = {}

        # icons change
        self.icon_open_inactive = QIcon(":/image/image/stop.png")
        self.icon_open_active = QIcon(":/image/image/start.png")
        self.icon_lock_inactive = QIcon(":/image/image/unlock.png")
        self.icon_lock_active = QIcon(":/image/image/lock.png")
        self.icon_3d_inactive = QIcon(":/image/image/2d.png")
        self.icon_3d_active = QIcon(":/image/image/3d.png")

        # status
        self.is_started = False
        self.is_locked = False
        self.is_3d = False

        # 性能监控窗口引用
        self.performance_monitor = None

        # 初始化后处理线程
        self.process_rgb_thread = None
        self.process_dp_thread = None
        self.process_pc_thread = None
        self.process_ir_thread = None
        self.save_thread = None

        """-------------------------------------------------------------------------------------"""
        # beautiful message box
        self.message_box_style = MessageBox_Style
        self.setup_message_box_style()

        """-------------------------------------------------------------------------------------"""
        # ui initial
        # scene and image item, for preview.
        self.scene_rgb = QGraphicsScene()
        self.scene_depth = QGraphicsScene()
        self.image_item_rgb = QGraphicsPixmapItem()
        self.image_item_depth = QGraphicsPixmapItem()

        self.setupUi(self)
        self.button_init()
        self.textbrowser_init()
        self._setup_lineedit_initialization()
        self._setup_combobox_with_data()
        self.view_init()

        """-------------------------------------------------------------------------------------"""
        self.img_rgb = None
        self.img_depth = None
        self.img_depth_color = None
        self.pointcloud = None

        """-------------------------------------------------------------------------------------"""
        # 安装事件过滤器（监听点击）
        self.view_rgb.viewport().installEventFilter(self)
        self.view_depth.viewport().installEventFilter(self)

        """-------------------------------------------------------------------------------------"""
        # bind logger
        self.logger.log_signal.connect(self.log_update)

        """-------------------------------------------------------------------------------------"""
        # 初始化 3D 视图（只创建一次）
        self.glw = CustomGLViewWidget()
        self.glw.setBackgroundColor((20, 20, 20))
        self.glw.opts['fov'] = 60
        self.glw.opts['elevation'] = 20
        self.glw.opts['azimuth'] = 45
        self.glw.opts['distance'] = 2.0

        # 可以动态调整灵敏度
        self.glw.mouse_sensitivity = 0.3  # 更低的值表示更慢的响应
        self.glw.pan_speed = 0.2  # 调整平移速度

        if not self.layout3D.layout():
            self.layout3D_layout = QtWidgets.QVBoxLayout(self.layout3D)
            self.layout3D_layout.setContentsMargins(0, 0, 0, 0)
            self.layout3D_layout.setSpacing(0)
        self.layout3D_layout.addWidget(self.glw)

        self.axis = gl.GLAxisItem()
        self.axis.setSize(x=0.5, y=0.5, z=0.5)  # 分别设置
        self.glw.addItem(self.axis)
        self.axis.hide()
        self.pc_item = gl.GLScatterPlotItem(size=2.0, pxMode=True)
        self.glw.addItem(self.pc_item)

        self._is_first_pointcloud = True

        """-------------------------------------------------------------------------------------"""
        self.val_fps = 20
        self.update_timer = QTimer()
        self.update_timer.timeout.connect(self.update_display)
        self.update_timer.start(33)  # 30fps

        self.show()

        # self._initial_key_data()
        QTimer.singleShot(20, self._start_initialization)

    def _start_initialization(self):
        self.logger.log(f" 🎉 [Info] Connect device and get calibration data...")

        self.progress_dialog = QProgressDialog("Connecting RMSL device...", "Cancel", 0, 100, self)
        self.progress_dialog.setStyleSheet(STYLE_CONFIG['label'] + STYLE_CONFIG['progressbar'])
        self.progress_dialog.setWindowModality(Qt.WindowModal)

        # 关键设置：进度满100时自动重置并关闭
        self.progress_dialog.setAutoClose(True)
        self.progress_dialog.setAutoReset(True)
        self.progress_dialog.setCancelButton(None)
        self.progress_dialog.setMinimumDuration(1)
        self.progress_dialog.setValue(0)

        self.comboBox_mode.setEnabled(False)
        self.comboBox_rgb_resolution.setEnabled(False)
        self.comboBox_disp_resolution.setEnabled(False)
        self.comboBox_ir_resolution.setEnabled(False)
        self.pushButton_performance.setEnabled(False)

        # 2. 启动线程
        self.controller = RMSLUVCController()
        self.init_thread = DeviceInitWorker(self.controller)

        # 3. 绑定信号：当线程结束时，调用 self._on_data_ready
        self.init_thread.progress_signal.connect(self.progress_dialog.setValue)
        self.init_thread.status_signal.connect(self.progress_dialog.setLabelText)
        self.init_thread.finished_signal.connect(self._on_data_ready)
        self.init_thread.start()  # 开始运行 run()

    def _on_data_ready(self, uvc_config, used_uvc_config, calib_info):
        if self.progress_dialog.isVisible():
            self.progress_dialog.close()

        self.uvc_config = uvc_config
        self.used_uvc_config = used_uvc_config
        self.calib_info = calib_info

        self.logger.log(f" 🎉 [Info] Success get device format list and calibration.")
        self.module_category = self.calib_info['category']
        self._update_combobox_and_lineedit_info()

    def _update_combobox_and_lineedit_info(self):
        prefix = self.used_uvc_config['rgb']['name'].split('_')[0]
        if 'sn_code' in self.calib_info:
            sn_code = f"{prefix}_RK-" + self.calib_info['sn_code']
            self.lineedit_device_code.setText(sn_code)

        # update rgb format list
        self.comboBox_rgb_resolution.clear()
        for config in self.uvc_config['rgb']:
            display_text = f"{config['width']}x{config['height']}@{int(config['fps'])}-{config['format'].lower()}"
            self.comboBox_rgb_resolution.addItem(display_text, config)
        self.comboBox_rgb_resolution.setCurrentIndex(0)

        # update depth format list
        self.comboBox_disp_resolution.clear()
        for config in self.uvc_config['depth']:
            display_text = f"{config['width']}x{config['height']}@{int(config['fps'])}-{config['format'].lower()}"
            self.comboBox_disp_resolution.addItem(display_text, config)
        self.comboBox_disp_resolution.setCurrentIndex(0)

        # update ir format list
        self.comboBox_ir_resolution.clear()
        for config in self.uvc_config['ir']:
            display_text = f"{config['width']}x{config['height']}@{int(config['fps'])}-{config['format'].lower()}"
            self.comboBox_ir_resolution.addItem(display_text, config)
        self.comboBox_ir_resolution.setCurrentIndex(0)

        self.comboBox_mode.setEnabled(True)
        self.comboBox_rgb_resolution.setEnabled(True)
        self.comboBox_disp_resolution.setEnabled(True)
        self.comboBox_ir_resolution.setEnabled(True)
        self.pushButton_performance.setEnabled(True)

    @staticmethod
    def resize_and_pad(img, target_h, target_w, en_rotate: bool = True):
        """
        将图像按比例缩放以适应 (target_h, target_w) 的尺寸，
        然后填充黑边使其居中。
        """
        if en_rotate:
            img = cv2.rotate(img, cv2.ROTATE_90_CLOCKWISE)
        original_h, original_w = img.shape[:2]

        # 1. 计算缩放比例
        # 看是高度限制了缩放，还是宽度限制了缩放
        ratio_h = target_h / original_h
        ratio_w = target_w / original_w
        scale_ratio = min(ratio_h, ratio_w)

        # 2. 计算缩放后的新尺寸
        new_w = int(original_w * scale_ratio)
        new_h = int(original_h * scale_ratio)

        # 3. 使用cv2.resize进行缩放
        # cv2.INTER_AREA 适合缩小图像，cv2.INTER_LINEAR 适合放大
        if scale_ratio < 1:
            interpolation = cv2.INTER_AREA
        else:
            interpolation = cv2.INTER_LINEAR

        # 注意：cv2.resize 的 dsize 参数是 (width, height)
        resized_img = cv2.resize(img, (new_w, new_h), interpolation=interpolation)

        # 4. 创建目标尺寸的黑色画布 (Pad)
        # 检查图像是彩色的 (3-channel) 还是灰度的 (1-channel 或 2-channel)
        if len(img.shape) == 3:
            # 创建一个 (h, w, c) 的画布
            padded = np.zeros((target_h, target_w, img.shape[2]), dtype=img.dtype)
        else:
            # 创建一个 (h, w) 的画布
            padded = np.zeros((target_h, target_w), dtype=img.dtype)

        # 5. 将缩放后的图像粘贴到画布中央
        # 计算粘贴位置 (左上角)
        y_offset = (target_h - new_h) // 2
        x_offset = (target_w - new_w) // 2

        padded[y_offset: y_offset + new_h, x_offset: x_offset + new_w] = resized_img

        return padded

    def update_display(self):
        if self.stream_mgr is None or not self.stream_mgr.stream_started:
            return

        img_dict = self.stream_mgr.get_ui_data()
        if img_dict['rgb'] is not None:
            self.img_rgb = img_dict['rgb']
            if self.en_debug:
                # show diff and ir images
                if img_dict['ir_left'] is not None and img_dict['ir_right'] is not None and img_dict['disp_diff'] is not None:
                    TARGET_H, TARGET_W = 540, 640
                    rgb_orig = self.img_rgb[::2, ::2]
                    ir_l_orig = img_dict['ir_left'][::2, ::2]
                    ir_r_orig = img_dict['ir_right'][::2, ::2]
                    diff_orig = img_dict['disp_diff'][::2, ::2]

                    rgb_ds = RMSLViewer.resize_and_pad(rgb_orig, TARGET_H, TARGET_W, en_rotate=True)
                    ir_l_ds = RMSLViewer.resize_and_pad(ir_l_orig, TARGET_H, TARGET_W, en_rotate=True)
                    ir_r_ds = RMSLViewer.resize_and_pad(ir_r_orig, TARGET_H, TARGET_W, en_rotate=True)
                    diff_ds = RMSLViewer.resize_and_pad(diff_orig, TARGET_H, TARGET_W, en_rotate=False)
                    top_row = np.hstack([rgb_ds, diff_ds])
                    bottom_row = np.hstack([ir_l_ds, ir_r_ds])
                    color_frame = np.vstack([top_row, bottom_row])
                else:
                    color_frame = self.img_rgb
            else:
                color_frame = self.img_rgb

            color_img = QImage(color_frame,
                               color_frame.shape[1],
                               color_frame.shape[0],
                               QImage.Format_RGB888)
            if not color_img.isNull():
                pixmap = QPixmap.fromImage(color_img)
                self.image_item_rgb.setPixmap(pixmap)
                self.image_item_rgb.setZValue(0)
                # 告诉 Scene 有效范围就是当前这张图片的范围
                self.scene_rgb.setSceneRect(QRectF(pixmap.rect()))

                self.show_rgb_frame()

        if not self.is_3d:
            if img_dict['depth_color'] is not None and img_dict['depth'] is not None:
                self.img_depth = img_dict['depth']
                self.img_depth_color = img_dict['depth_color']

            if self.img_depth_color is not None:
                depth_color = QImage(self.img_depth_color,
                                     self.img_depth_color.shape[1],
                                     self.img_depth_color.shape[0],
                                     QImage.Format_RGB888)

                if not depth_color.isNull():
                    pixmap = QPixmap.fromImage(depth_color)
                    self.image_item_depth.setPixmap(pixmap)
                    self.image_item_depth.setZValue(0)
                    # 告诉 Scene 有效范围就是当前这张图片的范围
                    self.scene_depth.setSceneRect(QRectF(pixmap.rect()))

                    self.show_depth_frame()
        else:
            if img_dict['pointcloud'] is not None:
                self.pointcloud = img_dict['pointcloud']

            # 3D mode
            self.update_pointcloud_display(self.pointcloud)

    def setup_message_box_style(self):
        QApplication.instance().setStyleSheet(QApplication.instance().styleSheet() + self.message_box_style)

    def log_update(self, msg):
        self.terminal_log.append(msg)
        scrollbar = self.terminal_log.verticalScrollBar()
        scrollbar.setValue(scrollbar.maximum())

    def button_init(self):
        self.pushButton_performance.setEnabled(True)
        self.pushButton_performance.clicked.connect(self.performance_tracking)

        self.pushButton_setting.setEnabled(True)
        self.pushButton_setting.clicked.connect(self.setting_params)

        self.pushButton_help.setEnabled(True)
        self.pushButton_help.clicked.connect(self.help_page)

        self.pushButton_start.setEnabled(True)
        self.pushButton_start.setIcon(self.icon_open_inactive)
        self.pushButton_start.clicked.connect(self.start_and_stop_process)

        self.pushButton_view_mode.setEnabled(True)
        self.pushButton_view_mode.setIcon(self.icon_3d_inactive)
        self.pushButton_view_mode.clicked.connect(self.view_mode_change)

        self.pushButton_lock.setEnabled(True)
        self.pushButton_lock.setIcon(self.icon_lock_inactive)
        self.pushButton_lock.clicked.connect(self.lock_and_unlock_view)

        self.pushButton_download.setEnabled(True)
        self.pushButton_download.clicked.connect(self.save_data)

    def performance_tracking(self):
        if self.performance_monitor is not None and self.performance_monitor.isVisible():
            self.performance_monitor.activateWindow()
            self.performance_monitor.raise_()
            return

        # 创建新窗口
        self.performance_monitor = PerformanceMonitorDialog(
            parent=self,
            config_info=self.config_info,
            calib_info=self.calib_info,
            stream_mgr=self.stream_mgr,
            device_mgr=None  # 如果有设备管理器
        )

        # 非模态窗口，不阻塞主窗口
        self.performance_monitor.show()

    def setting_params(self):
        """打开设置对话框"""
        dialog = SettingsDialog(self.config_info, self)

        if dialog.exec_() == QDialog.Accepted:
            self.config_info = dialog.config_info
            self.logger.log_to_file(f" 🎉 [Info] Config updated: {self.config_info}")

            if self.config_info['en_debug']:
                self.comboBox_mode.setCurrentIndex(2)
            else:
                self.comboBox_mode.setCurrentIndex(0)

    def help_page(self):
        help_msg = """
        <h3>工具使用说明</h3>
        <p>本工具为 RMSL 系列模组的 PC 端预览和分析平台，通过接入设备实时显示 RGB、深度伪彩图和其他数据流，并提供多种工具进行模式配置、算法分析和性能监控。界面功能分区如下：</p>

        <ul>
            <li><b>Config Panel（顶部区域）</b>：包含以下组件，按从左到右顺序排列：
                <ul>
                    <li><b>设备信息</b>：显示当前模组类型、序列号等基础信息</li>
                    <li><b>工作模式</b>：选择运行模式（如预览、标定、评估等）</li>
                    <li><b>RGB Format</b>：列出可用的 RGB 分辨率与帧率</li>
                    <li><b>Depth Format</b>：列出可用的深度输出格式</li>
                    <li><b>IR Format</b>：列出红外相机格式支持</li>
                    <li><b>Performance</b>：实时监控帧率、温度等指标</li>
                    <li><b>设置</b>：配置算法参数、数据保存格式、电气接口等</li>
                    <li><b>帮助</b>：查看当前页面说明（本提示）</li>
                </ul>
            </li>

            <li><b>实时显示区（中间区域）</b>：
                <br>用于显示 RGB 视频流和深度图/点云伪彩流。
            </li>

            <li><b>日志面板（底部左侧）</b>：
                <br>实时展示工具产生的运行日志和系统事件。
            </li>

            <li><b>快捷操作区（底部右侧）</b>：
                <ul>
                    <li>开启/关闭流</li>
                    <li>锁定当前画面</li>
                    <li>2D / 3D 显示模式切换</li>
                    <li>数据保存（图像、深度等）</li>
                </ul>
            </li>
        </ul>
        """
        QMessageBox.information(self, "Help", help_msg)

    def textbrowser_init(self):
        self.terminal_log.setReadOnly(True)
        self.terminal_log.setTextInteractionFlags(
            Qt.TextSelectableByMouse | Qt.TextSelectableByKeyboard
        )
        self.terminal_log.setOpenExternalLinks(True)
        self.terminal_log.setPlaceholderText(" ⏳ RMSLViewer Log terminal, waiting...")

    def view_init(self):
        self.view_rgb.setRenderHint(QPainter.SmoothPixmapTransform)
        self.view_rgb.setRenderHint(QPainter.Antialiasing)
        self.view_rgb.setOptimizationFlags(QGraphicsView.DontAdjustForAntialiasing)
        self.view_rgb.setOptimizationFlags(QGraphicsView.DontAdjustForAntialiasing | QGraphicsView.DontSavePainterState)
        self.view_rgb.setViewportUpdateMode(QGraphicsView.FullViewportUpdate)
        self.view_rgb.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
        self.view_rgb.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)

        self.view_depth.setRenderHint(QPainter.SmoothPixmapTransform)
        self.view_depth.setRenderHint(QPainter.Antialiasing)
        self.view_depth.setOptimizationFlags(QGraphicsView.DontAdjustForAntialiasing | QGraphicsView.DontSavePainterState)
        self.view_depth.setViewportUpdateMode(QGraphicsView.FullViewportUpdate)
        self.view_depth.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
        self.view_depth.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)

        # center visual
        self.view_rgb.centerOn(self.image_item_rgb)
        self.view_rgb.setAlignment(Qt.AlignCenter)
        self.view_depth.centerOn(self.image_item_depth)
        self.view_depth.setAlignment(Qt.AlignCenter)

    def _setup_combobox_with_data(self):
        self.comboBox_mode.setEnabled(True)
        self.comboBox_mode.clear()
        mode = ['Normal', 'Calibration', 'DEBUG']
        self.comboBox_mode.addItems(mode)

        if self.en_debug:
            self.comboBox_mode.setCurrentIndex(2)
        else:
            self.comboBox_mode.setCurrentIndex(0)
        self.comboBox_mode.currentTextChanged.connect(self.update_work_mode)

        # rgb
        self.comboBox_rgb_resolution.setEnabled(True)
        self.comboBox_rgb_resolution.clear()
        config_rgb = {'name': 'RMSL321_RGB', 'format': 'nv12', 'width': 1280, 'height': 1080, 'fps': 30.0}
        display_text = f"{config_rgb['width']}x{config_rgb['height']}@{int(config_rgb['fps'])}-{config_rgb['format'].lower()}"
        self.comboBox_rgb_resolution.addItem(display_text, config_rgb)
        self.comboBox_rgb_resolution.setCurrentIndex(0)
        self.comboBox_rgb_resolution.currentTextChanged.connect(self.update_uvc_rgb_config)

        # disp
        self.comboBox_disp_resolution.setEnabled(True)
        self.comboBox_disp_resolution.clear()

        config_disp = {'name': 'RMSL321_DEPTH', 'format': 'yuyv422', 'width': 640, 'height': 352, 'fps': 10.0}
        display_text = f"{config_disp['width']}x{config_disp['height']}@{int(config_disp['fps'])}-{config_disp['format'].lower()}"
        self.comboBox_disp_resolution.addItem(display_text, config_disp)

        self.comboBox_disp_resolution.setCurrentIndex(0)
        self.comboBox_disp_resolution.currentTextChanged.connect(self.update_uvc_disp_config)

        # ir
        self.comboBox_ir_resolution.clear()

        config_ir = {'name': 'RMSL321_IR', 'format': 'nv12', 'width': 1080, 'height': 1280, 'fps': 10.0}
        display_text = f"{config_ir['width']}x{config_ir['height']}@{int(config_ir['fps'])}-{config_ir['format'].lower()}"
        self.comboBox_ir_resolution.addItem(display_text, config_ir)

        self.comboBox_ir_resolution.setCurrentIndex(0)
        self.comboBox_ir_resolution.currentTextChanged.connect(self.update_uvc_ir_config)
        self.comboBox_ir_resolution.setEnabled(False)

    def update_work_mode(self, mode_text):
        if mode_text == 'DEBUG':
            self.en_debug = True
            self.config_info['en_debug'] = True
            self.comboBox_rgb_resolution.clear()

            # TODO: DEBUG
            if self.used_uvc_config['rgb'] is not None:
                cam_rgb_name = self.used_uvc_config['rgb']['name']
                config_rgb = {'name': cam_rgb_name, 'format': 'nv12', 'width': 1080, 'height': 1280, 'fps': 30.0}
            else:
                config_rgb = {'name': 'RMSL321_RGB', 'format': 'nv12', 'width': 1080, 'height': 1280, 'fps': 30.0}
            display_text = f"{config_rgb['width']}x{config_rgb['height']}@{int(config_rgb['fps'])}-{config_rgb['format'].lower()}"
            self.comboBox_rgb_resolution.addItem(display_text, config_rgb)
        else:
            self.en_debug = False
            self.config_info['en_debug'] = False
            self.comboBox_rgb_resolution.clear()
            if self.uvc_config['rgb'] is not None:
                for config in self.uvc_config['rgb']:
                    display_text = f"{config['width']}x{config['height']}@{int(config['fps'])}-{config['format'].lower()}"
                    self.comboBox_rgb_resolution.addItem(display_text, config)
            else:
                if self.used_uvc_config['rgb'] is not None:
                    cam_rgb_name = self.used_uvc_config['rgb']['name']
                    config_rgb = {'name': cam_rgb_name, 'format': 'nv12', 'width': 1280, 'height': 1080, 'fps': 30.0}
                else:
                    config_rgb = {'name': 'RMSL321_RGB', 'format': 'nv12', 'width': 1280, 'height': 1080, 'fps': 30.0}
                display_text = f"{config_rgb['width']}x{config_rgb['height']}@{int(config_rgb['fps'])}-{config_rgb['format'].lower()}"
                self.comboBox_rgb_resolution.addItem(display_text, config_rgb)

            # close ir
            self.comboBox_ir_resolution.setEnabled(False)
        self.logger.log(f" 🎉 [Info] Switch work mode to {mode_text}")

    def update_uvc_rgb_config(self, text):
        selected_config = self.comboBox_rgb_resolution.currentData()

        if selected_config:
            # update calib_info
            new_calib_info = CalibrationScaler.scale_intrinsic(
                calib_info=self.calib_info.copy(),
                target_width=selected_config['width'],
                target_height=selected_config['height'],
                camera_type='rgb'
            )
            self.calib_info = CalibrationScaler.update_alignment_coeffs(new_calib_info)

            # update stream_mgr
            self.used_uvc_config['rgb'] = selected_config
            self.logger.log(f" 🎉 [Info] Update UVC config: {selected_config['name']}: {selected_config['width']}x{selected_config['height']}@{int(selected_config['fps'])}-{selected_config['format'].lower()}")

    def update_uvc_disp_config(self, text):
        selected_config = self.comboBox_disp_resolution.currentData()

        if selected_config:
            # update calib_info
            new_calib_info = CalibrationScaler.scale_intrinsic(
                calib_info=self.calib_info.copy(),
                target_width=selected_config['width'],
                target_height=selected_config['height'],
                camera_type='depth'
            )
            self.calib_info = CalibrationScaler.update_alignment_coeffs(new_calib_info)

            # update stream_mgr
            self.used_uvc_config['depth'] = selected_config
            self.logger.log(f" 🎉 [Info] Update UVC config: {selected_config['name']}: {selected_config['width']}x{selected_config['height']}@{int(selected_config['fps'])}-{selected_config['format'].lower()}")

    def update_uvc_ir_config(self, text):
        selected_config = self.comboBox_ir_resolution.currentData()

        if selected_config:
            # update stream_mgr
            self.used_uvc_config['ir'] = selected_config
            self.logger.log(f" 🎉 [Info] Update UVC config: {selected_config['name']}: {selected_config['width']}x{selected_config['height']}@{int(selected_config['fps'])}-{selected_config['format'].lower()}")

    def _setup_lineedit_initialization(self):
        self.lineedit_device_code.setEnabled(True)
        self.lineedit_device_code.setReadOnly(True)
        self.lineedit_device_code.clear()

        prefix = 'RMSL32X'
        sn_code = f"{prefix}_RK-25042-1117"

        # Setting
        self.lineedit_device_code.setText(sn_code)

    def start_and_stop_process(self):
        if not self.is_started:
            # update icon
            self.pushButton_start.setIcon(self.icon_open_active)
            # load image to graphview
            self.start_view()
            self.is_started = True
        else:
            # update icon
            self.pushButton_start.setIcon(self.icon_open_inactive)
            # stop load image to graphview
            self.stop_view()
            self.is_started = False

    def start_view(self):
        # 关闭控制面板的使能
        self.comboBox_mode.setEnabled(False)
        self.comboBox_rgb_resolution.setEnabled(False)
        self.comboBox_disp_resolution.setEnabled(False)
        self.comboBox_ir_resolution.setEnabled(False)
        self.pushButton_setting.setEnabled(False)

        # 初始化流管理器
        self.stream_mgr = StreamManager(self.used_uvc_config, en_rgb_pointcloud=self.config_info.get('save_rgb_pointcloud', 0))
        if self.en_debug:
            self.stream_mgr.set_debug_mode(self.en_debug)

        self.logger.log(f" 🎉 [Info] Success initial stream manager.")

        # start
        self.stream_mgr.start()
        # rgb
        self.process_rgb_thread = ProcessRgbThread(self.stream_mgr, self.calib_info, self.config_info, en_undistort=False)
        self.process_rgb_thread.start()
        # disp
        self.process_dp_thread = ProcessDispThread(self.stream_mgr, self.calib_info, self.config_info)
        self.process_dp_thread.start()
        # pointcloud
        self.process_pc_thread = ProcessPointCloudThread(self.stream_mgr, self.calib_info, self.config_info)
        self.process_pc_thread.start()

        if self.en_debug == 1:
            self.process_ir_thread = ProcessIRThread(self.stream_mgr)
            self.process_ir_thread.start()

        self.save_thread = SaveDataThread(self.stream_mgr, self.save_dir, self.config_info)
        self.save_thread.start()
        self.logger.log(f" 🎉 [Info] Success start all streams.")

    def stop_view(self):
        # stop process thread
        self.save_thread.stop()
        if self.en_debug == 1:
            self.process_ir_thread.stop()

        self.process_pc_thread.stop()
        self.process_rgb_thread.stop()
        self.process_dp_thread.stop()

        self.stream_mgr.stop()

        # 打开控制面板的使能
        self.comboBox_mode.setEnabled(True)
        self.comboBox_rgb_resolution.setEnabled(True)
        self.comboBox_disp_resolution.setEnabled(True)
        self.comboBox_ir_resolution.setEnabled(True)
        self.pushButton_setting.setEnabled(True)
        self.logger.log(f" 🎉 [Info] Success close all stream and manager.")

    def view_mode_change(self):
        if not self.is_3d:
            # enable vis-pointcloud mode
            self.is_3d = True
            self.pushButton_view_mode.setIcon(self.icon_3d_active)
            self.depthStack.setCurrentIndex(1)
            self.stream_mgr.mode_3d = True
            self.logger.log(f" 🎉 [Info] Convert to 3D-PointCloud visual mode.")
        else:
            self.is_3d = False
            self.stream_mgr.mode_3d = False
            self.pushButton_view_mode.setIcon(self.icon_3d_inactive)
            self.depthStack.setCurrentIndex(0)

            self.logger.log(f" 🎉 [Info] Convert to 2D-Image visual mode.")

    def lock_and_unlock_view(self):
        if not self.is_locked:
            # close timer, not flush ui
            self.update_timer.stop()
            self.is_locked = True
            self.pushButton_lock.setIcon(self.icon_lock_active)
            self.logger.log(f" 🎉 [Info] Lock view.")
        else:
            self.update_timer.start(1000 / self.val_fps)
            self.is_locked = False
            self.pushButton_lock.setIcon(self.icon_lock_inactive)
            self.logger.log(f" 🎉 [Info] Unlock view.")

    def save_data(self):
        self.process_pc_thread.en_full_pointcloud_calculate(True)
        self.save_thread.set_save_flag(True)

    def update_pointcloud_display(self, pointcloud):
        if pointcloud is not None:
            if pointcloud.shape[1] == 3:
                pos, rgba = self.utils_pc.prepare_glscatter_data(pointcloud, colors=None)
            else:
                pos, rgba = self.utils_pc.prepare_glscatter_data(pointcloud)

            if pos.size:
                self.pc_item.setData(pos=pos, color=rgba)

                if self._is_first_pointcloud and len(pos) > 0:
                    min_bounds = np.min(pos, axis=0)
                    max_bounds = np.max(pos, axis=0)
                    center = (min_bounds + max_bounds) / 2
                    size = np.max(max_bounds - min_bounds)
                    center_vector = pg.Vector(center[0], center[1], center[2])

                    self.glw.setCameraPosition(
                        pos=center_vector,
                        distance=size * 1.0,
                        elevation=20,
                        azimuth=45
                    )

                    self._is_first_pointcloud = False
            else:
                # Clear
                self.pc_item.setData(pos=np.zeros((0, 3), dtype=np.float32), color=None)

    def show_depth_frame(self):
        if not self.view_depth.scene():
            self.scene_depth.addItem(self.image_item_depth)
        self.view_depth.setScene(self.scene_depth)
        self.view_depth.fitInView(self.image_item_depth, Qt.KeepAspectRatio)

    def show_rgb_frame(self):
        if not self.view_rgb.scene():
            self.scene_rgb.addItem(self.image_item_rgb)
        self.view_rgb.setScene(self.scene_rgb)
        self.view_rgb.fitInView(self.image_item_rgb, Qt.KeepAspectRatio)

    def resizeEvent(self, event):
        super().resizeEvent(event)
        if not self.image_item_rgb.pixmap().isNull():
            self.view_rgb.fitInView(self.image_item_rgb, Qt.KeepAspectRatio)
        if self.depthStack.currentWidget() is self.view2D:
            if not self.image_item_depth.pixmap().isNull():
                self.view_depth.fitInView(self.image_item_depth, Qt.KeepAspectRatio)

    def closeEvent(self, event):
        if hasattr(self, "save_thread") and self.save_thread:
            self.save_thread.stop()

        if hasattr(self, "process_pc_thread") and self.process_pc_thread:
            self.process_pc_thread.stop()

        if hasattr(self, "process_dp_thread") and self.process_dp_thread:
            self.process_dp_thread.stop()

        if hasattr(self, "process_rgb_thread") and self.process_rgb_thread:
            self.process_rgb_thread.stop()

        if self.en_debug == 1:
            if hasattr(self, "process_ir_thread") and self.process_ir_thread:
                self.process_ir_thread.stop()

        if self.stream_mgr is not None:
            self.stream_mgr.stop()
        super().closeEvent(event)
        event.accept()

    def eventFilter(self, obj, event):
        # if not self.is_locked:
        #     return super().eventFilter(obj, event)

        if event.type() == QEvent.MouseButtonPress and event.button() == Qt.LeftButton:
            if obj is self.view_rgb.viewport():
                if self._show_image_tooltip(self.view_rgb, self.image_item_rgb, event):
                    return True
            elif obj is self.view_depth.viewport():
                if self._show_image_tooltip(self.view_depth, self.image_item_depth, event, en_distance=True):
                    return True

        return super().eventFilter(obj, event)

    def _show_image_tooltip(self, view: QGraphicsView, item: QGraphicsPixmapItem, event, en_distance: bool = False):
        if item is None:
            return False

        pix = item.pixmap()
        if pix.isNull():
            return False

        vp_pos = event.pos()  # QPoint (Qt5)
        # 1) 命中测试：只在点到图像上时处理，避免留白区域误判
        hit_item = view.itemAt(vp_pos)
        if hit_item is not item:
            return False

        # 2) 坐标映射
        scene_pos = view.mapToScene(vp_pos)
        item_pos = item.mapFromScene(scene_pos)  # QPointF (item局部坐标，未缩放的逻辑坐标)

        # 3) 先用浮点做边界判断，避免 int 截断造成的负数变0
        dpr = pix.devicePixelRatio()  # HiDPI
        img_w = pix.width() / dpr
        img_h = pix.height() / dpr

        fx = item_pos.x()
        fy = item_pos.y()
        if not (0.0 <= fx < img_w and 0.0 <= fy < img_h):
            return False

        # 4) 再转像素坐标索引（向下取整）
        ix = int(fx * dpr)
        iy = int(fy * dpr)

        img = pix.toImage()
        # Qt5 一般有 pixelColor；如果你环境不支持，可改用 img.pixel + QColor
        if hasattr(img, "pixelColor"):
            c = img.pixelColor(ix, iy)
        else:
            rgb = img.pixel(ix, iy)
            c = QColor(rgb)

        # 5) 用 viewport 作为 widget & 坐标基准，避免窗口放大后 tooltip 定位异常
        gpos = view.viewport().mapToGlobal(vp_pos)
        if en_distance:
            if self.img_depth is None or self.img_depth.size == 0:
                QToolTip.showText(gpos,
                                  f"({int(fx)}, {int(fy)})  RGB: ({c.red()}, {c.green()}, {c.blue()})",
                                  view.viewport())
                return False

            depth_h, depth_w = self.img_depth.shape[:2]

            normalized_x = fx / img_w
            normalized_y = fy / img_h

            real_pos_x = int(normalized_x * depth_w)
            real_pos_y = int(normalized_y * depth_h)

            real_pos_x = min(real_pos_x, depth_w - 1)
            real_pos_y = min(real_pos_y, depth_h - 1)

            min_depth_dis = self.config_info.get('min_distance_mm', 100)  # mm
            max_depth_dis = self.config_info.get('max_distance_mm', 5000)  # mm
            distance = np.clip(self.img_depth[real_pos_y, real_pos_x], min_depth_dis, max_depth_dis)

            QToolTip.showText(gpos,
                              f"({int(fx)}, {int(fy)})  RGB: ({c.red()}, {c.green()}, {c.blue()})  Distance: {distance:.2f} mm",
                              view.viewport())
        else:
            QToolTip.showText(gpos,
                              f"({int(fx)}, {int(fy)})  RGB: ({c.red()}, {c.green()}, {c.blue()})",
                              view.viewport())
        return True

if __name__ == '__main__':
    QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True)
    QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True)

    if hasattr(Qt, 'HighDpiScaleFactorRoundingPolicy'):
        QApplication.setHighDpiScaleFactorRoundingPolicy(
            Qt.HighDpiScaleFactorRoundingPolicy.PassThrough)

    app = QApplication(sys.argv)
    app.setStyleSheet("""
        QToolTip {
            background-color: rgb(230, 230, 230);
            color: black;
            border: None;
            font-family: "YaHei Consolas Hybrid";
            font-size: 8pt;
        }
        """)

    demo = RMSLViewer()
    app.exec_()
